/*#pragma DATA_SECTION(imm, "SDRAMB");
unsigned short imm[6497]; 
unsigned short *im=&imm[0]; */
#include <stdio.h>

int counter=0;
int turning_left=0;
int turning_right=0;
int approaching_left=0;
int approaching_right=0;
int yprev;
int extradist=100;
int change;
int prevseg[4]={178,0,178,150};
int *prev=&prevseg[0];

void detection(unsigned short *image, unsigned short * im)
{
//printf("detection\n");
  double data[20];
  double* datapointer;
  //int* imagepointer;
  int ytarget=200;
  //int prevy=180;                   NOT USED?
  //unsigned short imm[6497];
  
  threshfuncv2(image,im);
  datapointer=&data[0];
  //printf("we get here\n");
  getxtarget(datapointer,ytarget,prev,im);
  /*
  if(data[2]==0){//no splits detected
    control(data[0],data[1],0,500);
  }
  if((turning_left==1)||(turning_right==1)){
    if((data[2]==-1)||(abs(90-data[3])>10)){//no vertical line or not yet aligned
      	if(turning_left){
	  control(0,0,1,500);
	}
	if(turning_right){
	  control(0,0,2,500);
	}
    }
    else{//return to tracking mode
      turning_right=0;
      turning_left=0;
      control(data[1],data[2],0,500);
    }
  }
  else
  {
    if(data[2]==1)    //sharp turn detected
	  {
      change=data[4]-yprev;
      approaching_left=1;
      control(data[4],data[5],0,500);//steer towards intersection
      prevy=data[4];
	  }
    if(data[2]==2)//sharp turn detected
	{
      change=data[4]-yprev;
      approaching_right=1;
      control(data[4],data[5],0,500);//steer towards intersection
      prevy=data[4];
	  
    }
    if(data[2]==-1){//no vertical line
      if(approaching_left||approaching_right){
        if(extradist>0){
	  control(178,200,0,500);
	  extradist=extradist-change;
        }
        else{
	  extradist=100;//reset
	  if(approaching_right==1){
	    turning_right==1;
	    approaching_right==0;
	  }
	  if(approaching_left==1){
	    turning_left==1;
	    approaching_left==0;
	  }
        }
      }
    }
}*/
control(178,200,0,700);

}



    